/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.worldobjects;

import static java.lang.Math.atan;

import java.util.LinkedList;

import robocup.component.geometry.Circle;
import robocup.component.geometry.StraightLine;
import robocup.component.geometry.Vektor;

/**
 * A line marks the border of the field.
 */
public class Line {

	/**
	 * @return returns a new Line-Object
	 */
	public static Line getDummyLine() {
		return new Line(new Vektor(0, 0), new Vektor(1, 90));
	}

	/**
	 * The cycle, in which the line was seen last.
	 */
	private int cycle;

	/**
	 * The unique id of the line.
	 */
	private int id;

	/**
	 * The first point defining the line.
	 */
	private Vektor point1;

	/**
	 * The second point defining the line.
	 */
	private Vektor point2;

	/**
	 * Line defined by 2 points.
	 * 
	 * @param iPoint1
	 * @param iPoint2
	 */
	public Line(Vektor iPoint1, Vektor iPoint2) {
		this.point1 = iPoint1;
		this.point2 = iPoint2;
	}

	/**
	 * Fieldline
	 * 
	 * @param iCycle -
	 *            time, when seen last
	 * @param iId -
	 *            the line id
	 * @param iPoint1 -
	 *            start-point of line
	 * @param iPoint2 -
	 *            end-point of line
	 */
	public Line(int iCycle, int iId, Vektor iPoint1, Vektor iPoint2) {
		this.id = iId;
		this.cycle = iCycle;
		this.point1 = iPoint1;
		this.point2 = iPoint2;
	} // end of constructor Line(int iCycle, int iId, Vektor iPoint1, Vektor


	/**
	 * The length of a line as distance between the 2 line points.
	 * 
	 * @return the length
	 */
	public double getLength() {
		return this.point1.sub(this.point2).getLength();
	}

	/**
	 * @param l the line to intersect with
	 * @return the intersectionpoint of this line with l
	 */
	public Vektor getIntersectionPointWithLine(Line l) {
		Vektor p1 = this.point1;
		Vektor p2 = this.point2;
		Vektor p3 = l.point1;
		Vektor p4 = l.point2;
		double m1 = 0;
		double m2 = 0;
		double n1 = 0;
		double n2 = 0;
		double x = 0;
		double y = 0;
		if (p1.x == p2.x) {
			m1 = (p1.y - p2.y) / 0.01;
		} else {
			m1 = (p1.y - p2.y) / (p1.x - p2.x);
		}
		if (p3.x == p4.x) {
			m2 = (p3.y - p4.y) / 0.01;
		} else {
			m2 = (p3.y - p4.y) / (p3.x - p4.x);
		}
		n1 = p1.y - m1 * p1.x;
		n2 = p3.y - m2 * p3.x;
		if (m1 == m2) {
			x = (n1 - n2) / 0.01;
		} else {
			x = (n1 - n2) / (m2 - m1);
		}
		y = m1 * x + n1;
		return new Vektor(Vektor.XY, x, y);
	}

	/**
	 * moves point 2 in that way, that the line has length l.
	 * @param l makes the line having length l
	 * 
	 */
	public void setLength(double l) {
		if (l <= 0) {
			System.out.println("[Line] length must be bigger than 0");
			return;
		}
		Vektor actualDist = this.point2.sub(this.point1);
		this.point2.copy(this.point1.add(actualDist.setLength(l)));
	}

	/**
	 * sets point1
	 * @param p1 the point to set
	 * 
	 */
	public void setPoint1(Vektor p1) {
		this.point1 = p1;
	}

	/**
   * changes point1 and point2
	 * 
	 */
	public void changeDirection() {
		Vektor buf = new Vektor();
		buf.copy(this.point2);
		this.point2.copy(this.point1);
		this.point1.copy(buf);
	}

	/**
	 * sets point2
	 * @param p2 the point to set
	 * 
	 */
	public void setPoint2(Vektor p2) {
		this.point2 = p2;
	}

	/**
	 * copy point1
	 * @param p1 the value to copy
	 * 
	 */
	public void copyPoint1(Vektor p1) {
		this.point1.copy(p1);
	}

	/**
	 * @return the angle from point 1 to point 2
	 */
	public double getAngle() {
		return point1.getAngleTo(point2);
	}

	/**
	 * copy point2
	 * @param p2 the value to copy
	 * 
	 */
	public void copyPoint2(Vektor p2) {
		this.point2.copy(p2);
	}

	/**
	 * Gets the intersection points of the line and a circle.
	 * @param c the circle to intersect with 
	 * 
	 * @return the intersection points (0-2)
	 */
	public LinkedList<Vektor> getIntersectionPoints(Circle c) {
		return getPointsWithDistanceToPoint(c.getRadius(), c.getCenter());
	}

	/**
	 * Gets the intersection points of the line and a circle.
	 * 
	 * @param distance
	 *            The circle's radius.
	 * @param point
	 *            The circle's center.
	 * @return points
	 */
	public LinkedList<Vektor> getPointsWithDistanceToPoint(double distance,
			Vektor point) {
		LinkedList<Vektor> points = new LinkedList<Vektor>();
		Vektor p = getPointWithSmallestDistanceToPoint(point);
		Vektor pointToP = p.sub(point);
		double pointToPLength = pointToP.getLength();
		// no intersection
		if (pointToPLength > distance) {
			return points;
		}
		// tangent
		if (pointToPLength == distance) {
			points.add(p);
			return points;
		}
		// secant
		double alpha = Math.asin(pointToPLength / distance);
		double b = Math.cos(alpha) * distance;
		double anglePointToP = pointToP.getAngle();
		// a Vektor with length = 0 has no angle
		if (pointToP.getLength() == 0.0)
			anglePointToP = 90.0 + this.point1.sub(point2).getAngle();
		points.add(p.add(new Vektor(Vektor.POLAR, b, anglePointToP - 90)));
		points.add(p.add(new Vektor(Vektor.POLAR, b, anglePointToP + 90)));
		return points;
	}

  /**
   * Caluculates the point with the desired y-value.
   * 
   * @param y the desired y-value
   * @return returns the point on the line, where the y-value is y
   */
	public Vektor getPointWithYValue(double y) {
		if (this.point1.y == this.point2.y) {
			return null;
		}
		Vektor v = Vektor.getDummyInstance();
		this.getPointWithYValue(y, v);
		return v;
	}

  /**
   * Calculates the Point on the line with desired y-value.
   * Copies the result to v
   * 
   * @param y the desired y-value
   * @param v the container for result
   */
	public void getPointWithYValue(double y, Vektor v) {
		if (this.point1.y == this.point2.y) {
			v.setEmpty();
			return;
		}
		double m = (this.point1.y - this.point2.y)
				/ (this.point1.x - this.point2.x);
		double n = this.point1.y - this.point1.x * m;
		v.x = (y - n) / m;
		v.y = y;
	}

  /**
   * Caluculates the point with the desired x-value.
   * 
   * @param x the desired x-value
   * @return returns the point on the line, where the x-value is x
   */
	public Vektor getPointWithXValue(double x) {
		if (this.point1.x == this.point2.x) {
			return null;
		}
		Vektor v = Vektor.getDummyInstance();
		this.getPointWithXValue(x, v);
		return v;
	}

  /**
   * Calculates the Point on the line with desired x-value.
   * Copies the result to v
   * 
   * @param x the desired x-value
   * @param v the container for result
   */
	public void getPointWithXValue(double x, Vektor v) {
		if (this.point1.x == this.point2.x) {
			v.setEmpty();
			return;
		}
		double m = (this.point1.y - this.point2.y)
				/ (this.point1.x - this.point2.x);
		double n = this.point1.y - this.point1.x * m;
		v.x = x;
		v.y = m * x + n;
	}

	/**
	 * @param point -
	 *            Vektor absolute position
	 * @return Vektor - point on line with smallest distance to the specified
	 *         point <code>point</code>. TODO test
	 */
	public Vektor getPointWithSmallestDistanceToPoint(Vektor point) {
		// lot will be the shortest vector from the point to the line
		// (or from the line to the point ....)
		Vektor lot = this.point1.sub(this.point2);
		lot.rotate(90.0);
		lot.setLength(getDistanceToPoint(point));
		// there should be a formula that tell the direction of lot
		// but this should work as well .....
		if (getDistanceToPoint(lot.add(point)) > 0.001)
			lot.rotate(180);
		return lot.add(point);
	}

	/**
	 * @param point -
	 *            Vektor absolute position
	 * @return double - minimal distance from the specified point
	 *         <code>point</code> to line TODO test
	 */
	public double getDistanceToPoint(Vektor point) {

		Vektor p1ToPoint = this.point1.sub(point);
		Vektor u = this.point1.sub(this.point2);
		return Math.abs(p1ToPoint.cross(u) / u.getLength());
	}

	/**
	 * @param point
	 * @return <code>true</code>, if the given point lies on the line,
	 *         <code>false</code> otherwise.
	 */
	public boolean isOnLine(Vektor point) {
		return this.getDistanceToPoint(point) < 0.001;
	}

	/**
	 * @return the perpendicular bisector of the side (Mittelsenkrechte)
	 * 
	 */
	public StraightLine getPerpendicularBisector() {
		return new StraightLine(this.point1.getVektorBetween(this.point2),
				getRightAngle());// toDegrees(getRectAngle() + 90));
	}

	/**
	 * @return the absolute angle of the line
	 */
	private double getRightAngle() {
		double divisor = getGradient();
		if (divisor == 0)
			divisor = 0.00001; // Avoid dividing through zero
		return atan(-1 / getGradient());
	}

	/**
	 * @return the gradient of the line. If the line is parallel to the y-Axis,
	 *         a blur effect will be established.
	 */
	private double getGradient() {
		double e = 0;
		if (this.point1.x == this.point2.x)
			e = 0.00001; // Avoid dividing by 0
		return (this.point2.y - this.point1.y)
				/ (this.point2.x - this.point1.x + e);
	}

	// ************* Getters + Setters *************************

	/**
	 * Returns the id of the line.
	 * 
	 * @return id
	 */
	public int getId() {
		return this.id;
	} // end of method getId()

	/**
	 * Sets the id of the line.
	 * 
	 * @param id the id to set
	 */
	public void setId(int id) {
		this.id = id;
	}

	/**
	 * @return Returns the point1.
	 */
	public final Vektor getPoint1() {
		return this.point1;
	}

	/**
	 * @return Returns the point2.
	 */
	public final Vektor getPoint2() {
		return this.point2;
	}

	/**
	 * @return Returns the cycle.
	 */
	public int getCycle() {

		return this.cycle;
	}

	/**
	 * @param cycle
	 *            The cycle to set.
	 */
	public void setCycle(int cycle) {

		this.cycle = cycle;
	}

	/**
	 * (non-Javadoc)
	 * 
	 * @see java.lang.Object#toString()
	 */
	public String toString() {
		return point1 + " - " + point2;
	}

} // end of class Line
